/************************************************************************************************************************
*Editor: watter
*Time: 2016-10-27
*Func: Search for circle
*Note: Return Circle position and radias, bias, points
***********************************************************************************************************************/
#include  "Sick_V2.h"
extern void usart1Printf(char *fmt,...);
/*
int16_t ValidData_D[812] = 
	{811,655,714,1018,1426,1559,1480,1342,964,829,616,551,552,636,676,673,675,641,2084,2195,2231,2229,2231,2237,
	2243,2241,2248,2514,1027,1001,1004,1011,1002,997,1006,996,1003,1005,1002,996,1006,1000,997,1008,1007,1003,
	997,1007,1006,1004,1017,1007,1004,999,1010,1011,1009,1018,1024,1015,1011,1013,1025,1013,1014,1021,1021,1008,
	1012,1029,1025,1018,1022,1025,1026,1027,1028,1029,1030,1025,1033,1034,1034,1037,1037,1041,1039,1036,1039,
	1046,1041,1045,1048,1050,1049,1054,1046,1057,1054,1052,1065,1804,1803,1805,1811,1840,1837,1860,1961,2075,
	2064,2049,439,474,506,538,566,583,609,617,633,643,639,639,639,642,645,641,645,644,651,665,662,677,689,736,
	960,1052,1055,1055,1050,1043,1045,1041,1032,1020,1020,1012,1014,1017,1009,1006,998,990,980,974,971,964,969,
	955,956,956,971,990,972,971,965,944,846,405,351,344,340,338,344,340,350,339,353,343,352,345,353,361,351,350,
	364,363,367,360,368,363,362,368,359,365,365,368,370,370,370,462,647,716,733,741,739,735,733,726,725,725,727,
	732,723,733,725,728,714,715,711,701,702,704,704,701,702,693,697,698,689,687,683,690,681,673,675,685,681,673,
	673,672,668,660,653,654,646,647,640,634,388,301,284,280,282,275,273,274,275,265,268,271,264,266,262,264,261,
	265,267,261,260,257,256,256,258,255,259,256,263,255,260,261,259,257,257,258,260,255,257,263,259,261,257,264,
	258,258,257,257,258,261,259,265,261,266,264,264,264,265,264,271,271,270,272,278,272,281,285,279,298,847,902,
	916,921,913,919,917,919,912,912,909,903,901,902,901,894,895,891,893,892,889,890,886,894,887,790,342,320,316,
	308,304,309,299,299,292,295,293,289,290,287,288,290,288,288,286,285,286,285,286,284,286,288,284,285,283,284,
	282,285,286,285,287,287,283,288,287,285,285,286,288,290,286,291,290,291,289,292,294,293,293,294,298,299,300,
	303,300,300,307,312,314,314,146,82,58,1692,1698,1709,1709,1725,1719,1736,1742,1748,1755,1764,1773,1784,1796,
	1802,1809,1818,1826,1843,1845,1859,1864,1876,1888,1898,1899,1919,1931,1946,325,351,357,353,329,305,296,299,
	294,302,308,297,295,297,307,291,290,293,293,296,286,289,290,288,273,285,289,285,290,289,295,298,297,300,296,
	303,303,297,302,300,310,461,800,1000,1000,2653,2,2,612,2,2,2,614,613,614,606,592,594,2,588,2,584,581,571,580,
	574,571,567,575,584,575,573,562,566,566,561,561,560,555,549,554,549,541,548,548,540,540,540,546,539,543,531,
	545,537,536,534,534,540,532,532,533,521,529,525,527,524,522,523,522,517,524,510,524,515,517,518,514,518,514,
	511,514,515,520,514,513,504,514,508,513,513,507,509,507,512,503,505,497,494,498,497,2,2189,2208,2213,2216,
	2217,2221,2217,2216,2219,2225,2236,2237,2235,2237,2237,2230,2224,2227,2225,2229,2233,2231,2229,2228,2226,
	2232,2553,2603,2368,2366,2366,2182,2147,2163,2340,2364,2366,2374,2384,2407,2653,2722,2731,2735,2737,2751,
	2742,2752,2763,2761,2769,2772,2775,2781,2787,2945,4765,4776,4792,4795,4804,4816,4828,4837,4848,836,840,
	828,834,830,827,822,828,823,824,830,833,820,822,826,2491,4056,4017,3971,3930,3888,3847,3808,3766,3506,
	3300,3264,3229,907,839,1044,1059,3137,3151,3160,1199,965,960,959,957,955,916,933,944,936,925,907,913,
	927,928,919,929,924,911,912,917,906,909,901,889,805,786,784,711,536,367,345,346,342,343,339,337,333,
	333,331,330,331,332,328,327,323,324,325,321,321,326,323,321,321,324,325,319,318,309,308,317,309,310,
	310,310,323,329,313,321,322,322,325,326,323,326,327,329,329,333,330,331,331,337,338,341,341,347,356,
	386,406,437,427,410,413,438,443,433,426,431,434,437,432,433,426,428,425,433,433,422,420,414,396,373,
	375,378,383,397,429,489,535,2177,2166,568,622,636,631,613,620,627,630};
	*/
//Ratio:1:1000
int16_t Table_Cos[Max_DataNum] = {
707, 711, 715, 719, 723, 727, 731, 735, 739, 743, 747, 750, 
754, 758, 762, 766, 769, 773, 777, 780, 784, 788, 791, 795, 
798, 802, 805, 809, 812, 815, 819, 822, 825, 829, 832, 835, 
838, 841, 844, 848, 851, 854, 857, 860, 863, 866, 868, 871, 
874, 877, 880, 882, 885, 888, 890, 893, 896, 898, 901, 903, 
906, 908, 911, 913, 915, 918, 920, 922, 924, 927, 929, 931, 
933, 935, 937, 939, 941, 943, 945, 947, 949, 951, 952, 954, 
956, 957, 959, 961, 962, 964, 965, 967, 968, 970, 971, 973, 
974, 975, 976, 978, 979, 980, 981, 982, 983, 984, 985, 986, 
987, 988, 989, 990, 991, 991, 992, 993, 993, 994, 995, 995, 
996, 996, 997, 997, 997, 998, 998, 998, 999, 999, 999, 999, 
999, 999, 999, 999, 999, 999, 999, 999, 999, 999, 999, 998, 
998, 998, 997, 997, 997, 996, 996, 995, 995, 994, 993, 993, 
992, 991, 991, 990, 989, 988, 987, 986, 985, 984, 983, 982, 
981, 980, 979, 978, 976, 975, 974, 973, 971, 970, 968, 967, 
965, 964, 962, 961, 959, 958, 956, 954, 952, 951, 949, 947, 
945, 943, 941, 939, 937, 935, 933, 931, 929, 927, 925, 922, 
920, 918, 915, 913, 911, 908, 906, 903, 901, 898, 896, 893, 
891, 888, 885, 883, 880, 877, 874, 871, 868, 866, 863, 860, 
857, 854, 851, 848, 845, 841, 838, 835, 832, 829, 825, 822, 
819, 815, 812, 809, 805, 802, 798, 795, 791, 788, 784, 780, 
777, 773, 769, 766, 762, 758, 754, 750, 747, 743, 739, 735, 
731, 727, 723, 719, 715, 711, 707, 703, 698, 694, 690, 686, 
682, 677, 673, 669, 664, 660, 656, 651, 647, 642, 638, 633, 
629, 624, 620, 615, 611, 606, 601, 597, 592, 587, 583, 578, 
573, 568, 564, 559, 554, 549, 544, 539, 534, 530, 525, 520, 
515, 510, 505, 500, 495, 490, 484, 479, 474, 469, 464, 459, 
454, 448, 443, 438, 433, 428, 422, 417, 412, 406, 401, 396, 
390, 385, 380, 374, 369, 363, 358, 353, 347, 342, 336, 331, 
325, 320, 314, 309, 303, 298, 292, 287, 281, 275, 270, 264, 
259, 253, 247, 242, 236, 230, 225, 219, 213, 208, 202, 196, 
191, 185, 179, 173, 168, 162, 156, 150, 145, 139, 133, 127, 
122, 116, 110, 104, 98, 93, 87, 81, 75, 69, 64, 58, 
52, 46, 40, 35, 29, 23, 17, 11, 6, 0, -5, -11, 
-17, -23, -28, -34, -40, -46, -52, -57, -63, -69, -75, -81, 
-86, -92, -98, -104, -110, -115, -121, -127, -133, -138, -144, -150, 
-156, -161, -167, -173, -179, -184, -190, -196, -201, -207, -213, -219, 
-224, -230, -236, -241, -247, -252, -258, -264, -269, -275, -280, -286, 
-292, -297, -303, -308, -314, -319, -325, -330, -336, -341, -347, -352, 
-358, -363, -368, -374, -379, -385, -390, -395, -401, -406, -411, -417, 
-422, -427, -432, -438, -443, -448, -453, -458, -464, -469, -474, -479, 
-484, -489, -494, -499, -504, -509, -514, -519, -524, -529, -534, -539, 
-544, -549, -554, -558, -563, -568, -573, -578, -582, -587, -592, -596, 
-601, -606, -610, -615, -619, -624, -629, -633, -638, -642, -647, -651, 
-655, -660, -664, -668, -673, -677, -681, -686, -690, -694, -698, -702, 
-706, -710, -715, -719, -723, -727, -731, -735, -739, -742, -746, -750, 
-754, -758, -762, -765, -769, -773, -776, -780, -784, -787, -791, -794, 
-798, -801, -805, -808, -812, -815, -818, -822, -825, -828, -832, -835, 
-838, -841, -844, -847, -850, -853, -856, -859, -862, -865, -868, -871, 
-874, -877, -880, -882, -885, -888, -890, -893, -896, -898, -901, -903, 
-906, -908, -911, -913, -915, -918, -920, -922, -924, -927, -929, -931, 
-933, -935, -937, -939, -941, -943, -945, -947, -949, -950, -952, -954, 
-956, -957, -959, -961, -962, -964, -965, -967, -968, -970, -971, -972, 
-974, -975, -976, -978, -979, -980, -981, -982, -983, -984, -985, -986, 
-987, -988, -989, -990, -991, -991, -992, -993, -993, -994, -995, -995, 
-996, -996, -997, -997, -997, -998, -998, -998, -999, -999, -999, -999, 
-999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -999, -998, 
-998, -998, -997, -997, -997, -996, -996, -995, -995, -994, -993, -993, 
-992, -991, -991, -990, -989, -988, -987, -986, -985, -984, -983, -982, 
-981, -980, -979, -978, -977, -975, -974, -973, -971, -970, -968, -967, 
-966, -964, -962, -961, -959, -958, -956, -954, -952, -951, -949, -947, 
-945, -943, -941, -939, -937, -935, -933, -931, -929, -927, -925, -922, 
-920, -918, -916, -913, -911, -908, -906, -904, -901, -898, -896, -893, 
-891, -888, -885, -883, -880, -877, -874, -872, -869, -866, -863, -860, 
-857, -854, -851, -848, -845, -842, -838, -835, -832, -829, -826, -822, 
-819, -816, -812, -809, -805, -802, -798, -795, -791, -788, -784, -781, 
-777, -773, -770, -766, -762, -758, -755, -751, -747, -743, -739, -735, 
-731, -727, -723, -719, -715, -711, -707
};
int16_t Table_Sin[Max_DataNum] = {
-707, -702, -698, -694, -690, -686, -682, -677, -673, -669, -664, -660, 
-656, -651, -647, -642, -638, -633, -629, -624, -620, -615, -611, -606, 
-601, -597, -592, -587, -583, -578, -573, -568, -564, -559, -554, -549, 
-544, -539, -534, -529, -524, -520, -515, -510, -505, -500, -494, -489, 
-484, -479, -474, -469, -464, -459, -454, -448, -443, -438, -433, -427, 
-422, -417, -412, -406, -401, -396, -390, -385, -380, -374, -369, -363, 
-358, -352, -347, -342, -336, -331, -325, -320, -314, -309, -303, -297, 
-292, -286, -281, -275, -270, -264, -258, -253, -247, -241, -236, -230, 
-225, -219, -213, -207, -202, -196, -190, -185, -179, -173, -167, -162, 
-156, -150, -144, -139, -133, -127, -121, -116, -110, -104, -98, -93, 
-87, -81, -75, -69, -64, -58, -52, -46, -40, -34, -29, -23, 
-17, -11, -5, -0, 5, 11, 17, 23, 29, 34, 40, 46, 
52, 58, 63, 69, 75, 81, 87, 92, 98, 104, 110, 116, 
121, 127, 133, 139, 144, 150, 156, 162, 167, 173, 179, 184, 
190, 196, 202, 207, 213, 219, 224, 230, 236, 241, 247, 253, 
258, 264, 269, 275, 281, 286, 292, 297, 303, 308, 314, 319, 
325, 330, 336, 341, 347, 352, 358, 363, 369, 374, 379, 385, 
390, 395, 401, 406, 411, 417, 422, 427, 433, 438, 443, 448, 
453, 459, 464, 469, 474, 479, 484, 489, 494, 499, 504, 509, 
514, 519, 524, 529, 534, 539, 544, 549, 554, 559, 563, 568, 
573, 578, 582, 587, 592, 597, 601, 606, 610, 615, 620, 624, 
629, 633, 638, 642, 647, 651, 655, 660, 664, 669, 673, 677, 
681, 686, 690, 694, 698, 702, 706, 711, 715, 719, 723, 727, 
731, 735, 739, 743, 746, 750, 754, 758, 762, 765, 769, 773, 
777, 780, 784, 787, 791, 795, 798, 802, 805, 808, 812, 815, 
819, 822, 825, 828, 832, 835, 838, 841, 844, 847, 851, 854, 
857, 860, 863, 865, 868, 871, 874, 877, 880, 882, 885, 888, 
890, 893, 896, 898, 901, 903, 906, 908, 911, 913, 915, 918, 
920, 922, 924, 927, 929, 931, 933, 935, 937, 939, 941, 943, 
945, 947, 949, 950, 952, 954, 956, 957, 959, 961, 962, 964, 
965, 967, 968, 970, 971, 972, 974, 975, 976, 978, 979, 980, 
981, 982, 983, 984, 985, 986, 987, 988, 989, 990, 991, 991, 
992, 993, 993, 994, 995, 995, 996, 996, 997, 997, 997, 998, 
998, 998, 999, 999, 999, 999, 999, 999, 999, 999, 999, 999, 
999, 999, 999, 999, 999, 998, 998, 998, 997, 997, 997, 996, 
996, 995, 995, 994, 993, 993, 992, 991, 991, 990, 989, 988, 
987, 986, 985, 984, 983, 982, 981, 980, 979, 978, 976, 975, 
974, 973, 971, 970, 968, 967, 965, 964, 962, 961, 959, 958, 
956, 954, 952, 951, 949, 947, 945, 943, 941, 939, 937, 935, 
933, 931, 929, 927, 925, 922, 920, 918, 916, 913, 911, 908, 
906, 903, 901, 898, 896, 893, 891, 888, 885, 883, 880, 877, 
874, 871, 869, 866, 863, 860, 857, 854, 851, 848, 845, 841, 
838, 835, 832, 829, 825, 822, 819, 815, 812, 809, 805, 802, 
798, 795, 791, 788, 784, 780, 777, 773, 769, 766, 762, 758, 
754, 751, 747, 743, 739, 735, 731, 727, 723, 719, 715, 711, 
707, 703, 699, 694, 690, 686, 682, 677, 673, 669, 665, 660, 
656, 651, 647, 643, 638, 634, 629, 625, 620, 615, 611, 606, 
602, 597, 592, 588, 583, 578, 573, 569, 564, 559, 554, 549, 
544, 540, 535, 530, 525, 520, 515, 510, 505, 500, 495, 490, 
485, 480, 474, 469, 464, 459, 454, 449, 443, 438, 433, 428, 
422, 417, 412, 407, 401, 396, 391, 385, 380, 374, 369, 364, 
358, 353, 347, 342, 336, 331, 325, 320, 314, 309, 303, 298, 
292, 287, 281, 275, 270, 264, 259, 253, 247, 242, 236, 230, 
225, 219, 213, 208, 202, 196, 191, 185, 179, 174, 168, 162, 
156, 151, 145, 139, 133, 128, 122, 116, 110, 104, 99, 93, 
87, 81, 75, 70, 64, 58, 52, 46, 41, 35, 29, 23, 
17, 12, 6, 0, -5, -11, -17, -22, -28, -34, -40, -46, 
-51, -57, -63, -69, -75, -80, -86, -92, -98, -104, -109, -115, 
-121, -127, -133, -138, -144, -150, -156, -161, -167, -173, -178, -184, 
-190, -196, -201, -207, -213, -218, -224, -230, -235, -241, -247, -252, 
-258, -264, -269, -275, -280, -286, -291, -297, -303, -308, -314, -319, 
-325, -330, -336, -341, -347, -352, -357, -363, -368, -374, -379, -384, 
-390, -395, -401, -406, -411, -416, -422, -427, -432, -437, -443, -448, 
-453, -458, -463, -469, -474, -479, -484, -489, -494, -499, -504, -509, 
-514, -519, -524, -529, -534, -539, -544, -549, -553, -558, -563, -568, 
-573, -577, -582, -587, -592, -596, -601, -606, -610, -615, -619, -624, 
-628, -633, -637, -642, -646, -651, -655, -660, -664, -668, -673, -677, 
-681, -685, -690, -694, -698, -702, -706
};
//global data
EthDataQueue EthQueue;
Sick_Queue	Sick_Result;
DataQueue	  Data_Input;	
int16_t			QData[Max_DataNum];
uint16_t		QSn[Max_DataNum];
int16_t 		Points_X[Max_DataNum];
int16_t 		Points_Y[Max_DataNum];
uint16_t 		DivPoints[Max_DataNum];
//temp data 
Sick_Info  	Info_T;
Sick_Queue  Queue_T;
int16_t			Data_T[Max_DataNum];
uint16_t		Sn_T[Max_DataNum];
int32_t 		R2_t[128];
int16_t 	  Data_X[128], Data_Y[128];
int16_t 		Seg_Circ_X[80], Seg_Circ_Y[80], Seg_Circ_R2[80], Seg_Circ_R2_Bias[80];
/* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%数据区域选择**/
//%超过1000mm的排除
static void SearchArea_Mgt(DataQueue* Queue_In, int16_t MaxLen)
{
	uint16_t 		i, len, index;
//	int16_t			Data_T[Max_DataNum];
//	uint16_t		Sn_T[Max_DataNum];
	int16_t *Pdata_T;
	uint16_t *Psn_T;
	
	Pdata_T = Data_T;
	Psn_T = Sn_T;
	//data copy
	len = Queue_In->size;
	index = 0;
	for(i=0; i<len; i++)
	{
		if ((Queue_In->Queue.Data[i]<MaxLen)&&(Queue_In->Queue.Data[i]>0))
		{			
			//Queue_T->Queue[index].Data = Queue_In->Queue[i].Data;
			Pdata_T[index] =  Queue_In->Queue.Data[i];
			//Queue_T->Queue[index].Sn = Queue_In->Queue[i].Sn;
			Psn_T[index] =  Queue_In->Queue.Sn[i];			
			//usart1Printf("index->%d, Sn->%d, data->%d \n", index, Sn_T[index], Data_T[index]);
			index++;
		}
	}
	//Queue_T->size = index;
	//data update
	len = index;
	Queue_In->size = index;
	for(i=0; i<index; i++)
	{
		Queue_In->Queue.Data[i] = Pdata_T[i];
		Queue_In->Queue.Sn[i] = Psn_T[i];
		//usart1Printf("index->%d, Sn->%d, data->%d \n", i, Queue_In->Queue.Sn[i], Queue_In->Queue.Data[i]);
	}
	//usart1Printf("index->%d, Sn->%d, data->%d \n", i-1, Queue_In->Queue.Sn[i-1], Queue_In->Queue.Data[i-1]);
}
// % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%数据连续性检测——数据分割
// % 阈值设定
// % 0-100:2*0.1*1000*sin(0.167)*5;
// % 100-200:2*0.2*1000*sin(0.167)*4;
// % 200-500:2*0.5*1000*sin(0.167)*3;
// % 500-1000:2*1.0*1000*sin(0.167)*2;
static void Continue_Mgt(DataQueue* Data_In)
{
	int16_t Offset_T;
	int8_t  Offset_0To100, Offset_100To200, Offset_200To500, Offset_500To1000;
	uint16_t i, len, index;

	Offset_0To100=3;//2.91
	Offset_100To200=4;//3.49
	Offset_200To500=9;//8.74
	Offset_500To1000=12;//11.65
	
	len = Data_In->size;
	Data_In->Div_Len = 1;
	//usart1Printf("len->%d \n", len);
	// % 指示第一个分段元素的首位置
	Data_In->DivPoints[0] = 0;
	index = 1;
	for	(i=1; i<len;i++)
	{
		Offset_T = (Data_In->Queue.Data[i]>Data_In->Queue.Data[i-1])?
		           (Data_In->Queue.Data[i]-Data_In->Queue.Data[i-1]):
		           (Data_In->Queue.Data[i-1]-Data_In->Queue.Data[i]); 
		// %小于100mm范围内数据分割     
		if (Data_In->Queue.Data[i]<100)
		{
			if(Offset_T>=Offset_0To100)
			{
				Data_In->DivPoints[index++] = i;
				Data_In->Div_Len++;
				//usart1Printf("Offset->%d, Point->%d, Len->%d \n", Offset_T, Data_In->DivPoints[index-1], Data_In->Div_Len);
			}
		}
		// %100mm-200mm范围内数据分割  
		else if(Data_In->Queue.Data[i]<200)
		{
			if(Offset_T>=Offset_100To200)
			{
				Data_In->DivPoints[index++] = i;
				Data_In->Div_Len++;
				//usart1Printf("Offset->%d, Point->%d, Len->%d \n", Offset_T, Data_In->DivPoints[index-1], Data_In->Div_Len);
			}
		}
		// %200mm-500mm范围内数据分割  
		else if(Data_In->Queue.Data[i]<500)
		{
			if(Offset_T>=Offset_200To500)
			{
				Data_In->DivPoints[index++] = i;
				Data_In->Div_Len++;
				//usart1Printf("Offset->%d, Point->%d, Len->%d \n", Offset_T, Data_In->DivPoints[index-1], Data_In->Div_Len);
			}
		}
		// %500mm-1000mm范围内数据分割  
		else if(Data_In->Queue.Data[i]<1000)
		{
			if(Offset_T>=Offset_500To1000)
			{
				Data_In->DivPoints[index++] = i;
				Data_In->Div_Len++;
				//usart1Printf("Offset->%d, Point->%d, Len->%d \n", Offset_T, Data_In->DivPoints[index-1], Data_In->Div_Len);
			}
		}
	}
	
	// % 最后一个元素定位
	if(Data_In->DivPoints[index-1]<(len-1)){
		Data_In->DivPoints[index++] = len-1;
		Data_In->Div_Len++;
		//usart1Printf("Point->%d, Len->%d \n", Data_In->DivPoints[index-1], Data_In->Div_Len);
	}
	//usart1Printf("index->%d, Points->%d, Len->%d \n", index-1, Data_In->DivPoints[index-1], Data_In->Div_Len);
}

// % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%有效数据段统计
// %每个分段数据大于30
//[SegmentData_Valid, SegmentData_Valid_a, Valid_Index_List, List_L] ...,
//    = SegDataValidSet(Data_Less1000_L,Data_Less1000_a, Index_List, segmin);
static void Valid_SegMent_Mgt(DataQueue* Data_In, uint16_t minnum)
{
	uint16_t i, j, len, index_t1, index_t2, index;
	
	Data_In->Seg_Len = 0;
	index = 0;
	len = Data_In->Div_Len;
	for(i=1; i<len; i++)
	{
		if(Data_In->DivPoints[i]>Data_In->DivPoints[i-1])
		{
			if((Data_In->DivPoints[i]-Data_In->DivPoints[i-1])<30)
			{
				// %无效数据清零
				index_t1 = Data_In->DivPoints[i-1];  
				index_t2 = Data_In->DivPoints[i];
				for(j=index_t1;j<(index_t2-1); j++)
				{
					Data_In->Queue.Data[j] = 0;
					Data_In->Queue.Sn[j] = 0;
				}
			}
			else  
			{
				// %有效数据绘图
				Data_In->SegPoints[index].Sn_S = Data_In->DivPoints[i-1];//-1;
				Data_In->SegPoints[index].Sn_E = Data_In->DivPoints[i];
				index++;	
//				usart1Printf("index->%d, Start->%d, Len->%d \n", 
//				             index-1, 
//				             Data_In->SegPoints[index-1].Sn_S, 
//										 Data_In->SegPoints[index-1].Sn_E);				
			}	
		}
	}
	Data_In->Seg_Len = index;
	//usart1Printf("SegLen->%d \n", index);				
}

//int32_t  Temp, temp_T, result_x, result_y;
void CoordinateConvert_Mgt(DataQueue* DataQueue_T)
{
  // % 坐标变换(R,a)->(x,y)
	int32_t  Temp, temp_T;
	uint16_t len, i, index;
	int16_t  data_t;	
	len = DataQueue_T->size;
	//usart1Printf("Len->%d \n", len);	
	for (i=0;i<len; i++)
	{
		//usart1Printf("i->%d \n", i);	
		if(DataQueue_T->Queue.Data[i]>0)
		{
			index = DataQueue_T->Queue.Sn[i];
			data_t = (int16_t)Table_Cos[index];
			//usart1Printf("data->%d \n", data_t);	
			temp_T = DataQueue_T->Queue.Data[i];
			Temp = data_t*temp_T/1000;
			DataQueue_T->XY_Queue.X[i] = Temp;


			//result_x = DataQueue_T->XY_Queue.X[i];
			
			data_t = (int16_t)Table_Sin[index];
			temp_T = DataQueue_T->Queue.Data[i];
			Temp = data_t*temp_T/1000;			
			DataQueue_T->XY_Queue.Y[i] = Temp;
			//result_y = DataQueue_T->XY_Queue.Y[i];			
//			usart1Printf("i->%d, Data->%d, a->%d, Index->%d, X->%d, Y->%d \n", 
//										i, DataQueue_T->Queue.Data[i], DataQueue_T->Queue.Sn[i],
//										index, DataQueue_T->XY_Queue.X[i], DataQueue_T->XY_Queue.Y[i]);	
		}
	}
  //usart1Printf("i->%d \n", i);	
}

/***************************************************************************?
*para: 
	bias, ratio->1:1000
	expect_bias, ratio->1:1000
***************************************************************************/
static void CircleCentreGet(int16_t* data_x_in, int16_t* data_y_in, uint16_t len, 
	                          int16_t expect_bias, Sick_Info* Info_T)
{
	// % 输入坐标数组(x,y)
	// % 如果输入的数组符合圆特征，输出对应的半径，数据中心的坐标，圆心和结果响应
	int16_t data_a[2];
	int16_t data_b[2];
	int16_t data_c[2];
	int16_t data_d[2];
	int16_t data_e[2];
	int16_t mid_point[2];
	int32_t k1, k2, b1, b2;
	int16_t centre_x, centre_y;
//	int32_t R2_t[128];
	int32_t R2_mean;
	int16_t count, offset, len_2;
	int32_t *Pr2_t;
	
	uint16_t	i;
	
	Pr2_t = R2_t;
	// % 基准点坐标
	// % 起点
	data_a[0] = data_x_in[0];
	data_a[1] = data_y_in[0];
	// % 中点
	i = (uint8_t)(len/2);
//	usart1Printf("i->%d, len->%d \n", i, len);
	data_b[0] = data_x_in[i-1];
	data_b[1] = data_y_in[i-1];
	// % 末点
	data_c[0] = data_x_in[len-1];
	data_c[1] = data_y_in[len-1];
	// % 前半段中点
	data_d[0] = (data_a[0]+data_b[0])/2;
	data_d[1] = (data_a[1]+data_b[1])/2;
	// % 后半段中点
	data_e[0] = (data_c[0]+data_b[0])/2;
	data_e[1] = (data_c[1]+data_b[1])/2;
	// % 中心点坐标
	mid_point[0] = (data_a[0]+data_c[0])/2;
	mid_point[1] = (data_a[1]+data_c[1])/2;

	if (len>=3)
	{
		// %圆心垂直于a-b直线斜率1, ratio 1:100
		k1 = 0 - 1000*(data_b[0]-data_a[0])/(data_b[1]-data_a[1]);
		// %圆心垂直于b-c直线斜率2, ratio 1:10
		k2 = 0 - 1000*(data_c[0]-data_b[0])/(data_c[1]-data_b[1]);
		// % 圆心垂直于a-b直线 参数
		b1 = 1000*data_d[1] - k1*data_d[0];
		// % 圆心垂直于a-c直线 参数
		b2 = 1000*data_e[1] - k2*data_e[0];
		// %理论圆心坐标
		centre_x = (b2-b1)/(k1-k2);
		centre_y = (k1*centre_x+b1)/1000;
//    usart1Printf("data_a(%d, %d), data_b(%d, %d), data_c(%d, %d), data_d(%d, %d), data_e(%d, %d) \n",
//             data_a[0], data_a[1], data_b[0], data_b[1], data_c[0], data_c[1],data_d[0], data_d[1],data_e[0], data_e[1]);
//    usart1Printf("k1->%d, k2->%d, b1->%d, b2->%d \n", k1, k2, b1, b2);
		// %理论半径平方
		len_2 = 0;
		R2_mean = 0;
//		usart1Printf("centre_x->%d, centre_y->%d \n", centre_x, centre_y);
		for (i=0;i<len;i++)
		{
			Pr2_t[i] = (centre_x-data_x_in[i])*(centre_x-data_x_in[i]);
			Pr2_t[i] = Pr2_t[i]+(centre_y-data_y_in[i])*(centre_y-data_y_in[i]);
			Pr2_t[i] = Pr2_t[i]/100;//cm^2
			//usart1Printf("R2_t->%d \n",  R2_t[i]);	
			R2_mean = R2_mean+Pr2_t[i];//cm^2	
			len_2++;
			//usart1Printf("(%d, %d); ", data_x_in[i], data_y_in[i]);
			//usart1Printf("x->%d, y->%d, R2_t(cm^2)->%d; ", data_x_in[i], data_y_in[i], Pr2_t[i]);	
		}
		//usart1Printf(" \n");	
		R2_mean = R2_mean/len_2;
//		usart1Printf("R2_mean(cm^2)->%d \n",  R2_mean);	
		count = 0;
		for (i=0;i<len;i++)
		{
			offset = (Pr2_t[i]>R2_mean)?(Pr2_t[i]-R2_mean):(R2_mean-Pr2_t[i]);
			offset = 100*offset/R2_mean;
			if(offset<=expect_bias)
			{
				count = count+1;
			}		
		}
		if(count>(len_2*2/3))
		{
//			Info_T->result = 1;
			Info_T->circ_x = centre_x;
			Info_T->circ_y = centre_y;
			Info_T->circ_r2 = R2_mean;
			Info_T->circ_r2_bias = 100*count/len_2;
			Info_T->circ_points = count;
			Info_T->mid_x = mid_point[0];
			Info_T->mid_y = mid_point[1];
		}else{
//			Info_T->result = 0;
			Info_T->circ_x = 0;
			Info_T->circ_y = 0;
			Info_T->circ_r2 = 0;
			Info_T->circ_r2_bias = 0;
			Info_T->circ_points = 0;
			Info_T->mid_x = 0;
			Info_T->mid_y = 0;
		}
//		usart1Printf("circ_x->%d, circ_y->%d, circ_r2->%d, circ_r2_bias->%d \n", 
//									Info_T->circ_x, Info_T->circ_y, Info_T->circ_r2, Info_T->circ_r2_bias);	
	}
}
/*
Result: 0->circ_r, 1->circ_x, 2->circ_y
*/
static void leastSquare_circ(int16_t* data_x_in, int16_t* data_y_in, uint16_t len, int32_t Result[])
{
//% 基于数据圆弧拟合
	uint16_t i;
	int64_t x1, x2, x3, y1, y2, y3, x1y1, x1y2, x2y1;
	int64_t C, D, E, G, H;
	int64_t a, b, c;
	x1 = 0; x2 = 0; x3 = 0;
	y1 = 0; y2 = 0; y3 = 0;
	x1y1 = 0; x1y2 = 0; x2y1 = 0;
	for(i=0; i<len; i++){
    x1 = x1 + data_x_in[i];
    x2 = x2 + data_x_in[i]*data_x_in[i];
    x3 = x3 + data_x_in[i]*data_x_in[i]*data_x_in[i];
    y1 = y1 + data_y_in[i];
    y2 = y2 + data_y_in[i]*data_y_in[i];
    y3 = y3 + data_y_in[i]*data_y_in[i]*data_y_in[i];
    x1y1 = x1y1 + data_x_in[i]*data_y_in[i];
    x1y2 = x1y2 + data_x_in[i]*data_y_in[i]*data_y_in[i];
    x2y1 = x2y1 + data_x_in[i]*data_x_in[i]*data_y_in[i];
	}

	C = len * x2 - x1 * x1;
	D = len * x1y1 - x1 * y1;
	E = len * x3 + len * x1y2 - (x2 + y2) * x1;
	G = len * y2 - y1 * y1;
	H = len * x2y1 + len * y3 - (x2 + y2) * y1;

	a = (H * D - E * G)/(C * G - D * D);
	b = (H * C - E * D)/(D * D - G * C);
	c = -(a * x1 + b * y1 + x2 + y2)/len;

	Result[1] = a/(-2);          //x坐标
	Result[2] = b/(-2);          //y坐标
	Result[0] = (a * a + b * b - 4 * c)/4;
}

static void leastsquare_circget(int16_t* data_x_in, int16_t* data_y_in, uint16_t len, 
	                          int16_t expect_bias, Sick_Info* Info_T)
{
	// % 输入坐标数组(x,y)
	// % 如果输入的数组符合圆特征，输出对应的半径，数据中心的坐标，圆心和结果响应
	int16_t data_a[2];
	int16_t data_c[2];
	int16_t mid_point[2];
	int32_t Circ_Result[3];
	int32_t k1, k2, b1, b2;
	int16_t centre_x, centre_y;
//	int32_t R2_t[128];
	int32_t R2_mean;
	int16_t count, offset, len_2;
	int32_t *Pr2_t;
	
	uint16_t	i;
	
	Pr2_t = R2_t;
	// % 基准点坐标
	// % 起点
	data_a[0] = data_x_in[0];
	data_a[1] = data_y_in[0];
	// % 末点
	data_c[0] = data_x_in[len-1];
	data_c[1] = data_y_in[len-1];
	// % 中心点坐标
	mid_point[0] = (data_a[0]+data_c[0])/2;
	mid_point[1] = (data_a[1]+data_c[1])/2;
  //circ get
	leastSquare_circ( data_x_in,  data_y_in, len, Circ_Result);
	R2_mean = Circ_Result[0];
	centre_x = (int16_t)Circ_Result[1];
	centre_y = (int16_t)Circ_Result[2];
//	usart1Printf("r2->%d, x->%d, y->%d  \n", R2_mean, centre_x, centre_y);
	len_2 = 0;
	for (i=0;i<len;i++)
	{
		Pr2_t[i] = (centre_x-data_x_in[i])*(centre_x-data_x_in[i]);
		Pr2_t[i] = Pr2_t[i]+(centre_y-data_y_in[i])*(centre_y-data_y_in[i]);
		len_2++;		
		//usart1Printf("x->%d, y->%d, R2_t(cm^2)->%d; ", data_x_in[i], data_y_in[i], Pr2_t[i]);	
	}
	count = 0;
	for (i=0;i<len;i++)
	{
		offset = (Pr2_t[i]>R2_mean)?(Pr2_t[i]-R2_mean):(R2_mean-Pr2_t[i]);
		offset = 100*offset/R2_mean;
		if(offset<=expect_bias)
		{
			count = count+1;
		}		
		//usart1Printf("(ri2->%d, r2->%d), count->%d; ", Pr2_t[i], R2_mean, count);
	}
	//usart1Printf("count->%d, \n ", count);
	if(count>(len_2*2/3))
	{
//			Info_T->result = 1;
		Info_T->circ_x = centre_x;
		Info_T->circ_y = centre_y;
		Info_T->circ_r2 = R2_mean/100;//cm^2
		Info_T->circ_r2_bias = 100*count/len_2;
		Info_T->circ_points = count;
		Info_T->mid_x = mid_point[0];
		Info_T->mid_y = mid_point[1];
	}else{
//			Info_T->result = 0;
		Info_T->circ_x = 0;
		Info_T->circ_y = 0;
		Info_T->circ_r2 = 0;
		Info_T->circ_r2_bias = 0;
		Info_T->circ_points = 0;
		Info_T->mid_x = 0;
		Info_T->mid_y = 0;
	}
}
	
	
/*******************************************
//圆弧特征提取
*return : search result by a struct
*******************************************/
static void SegDataCircSearch(uint16_t index_s, uint16_t index_e, uint8_t offset, 
	                            DataQueue* DataQueue_T, Sick_Queue* Queue_T)
{
	Sick_Info  	*Pinfo;
	int16_t   *Px, *Py;
//	int16_t 	Data_X[128], Data_Y[128];	
	int16_t 	xx_t, yy_t, expect_bias;
	uint16_t 	Seg_Circ_i, SegData_List_i, j, k;
	uint32_t 	circle_l, mid_l;
	
	expect_bias = 20;
	Px = Data_X;
	Py = Data_Y;
	Pinfo = &Info_T;
	
	//圆弧特征提取
	Queue_T->size = 0;
	Seg_Circ_i = 0;
	for (j=index_s;j<(index_e);j=(j+offset))
	{
	  SegData_List_i = 0;
	  if((j+2*offset)<=index_e)
	  {
			for (k=j;k<(j+offset); k++)
			{
				Px[SegData_List_i] = DataQueue_T->XY_Queue.X[k];
				Py[SegData_List_i] = DataQueue_T->XY_Queue.Y[k];
				SegData_List_i = SegData_List_i+1;
			}
			//CircleCentreGet(Px, Py, offset, expect_bias, Pinfo); 
			leastsquare_circget(Px, Py, offset, expect_bias, Pinfo); 
	  }
	  else
	  {
			for (k=j; k<(index_e); k++)
			{
				Px[SegData_List_i] = DataQueue_T->XY_Queue.X[k];
				Py[SegData_List_i] = DataQueue_T->XY_Queue.Y[k];
				SegData_List_i = SegData_List_i+1;
			}
			//CircleCentreGet(Px, Py, index_e-j, expect_bias, Pinfo);
			leastsquare_circget(Px, Py, index_e-j, expect_bias, Pinfo);
	  }                 
//		usart1Printf("index->%d, circ_x->%d, circ_y->%d, circ_r2(cm^2)->%d \n", 
//									Seg_Circ_i, Pinfo->circ_x, Pinfo->circ_y, Pinfo->circ_r2);	
	  // %圆弧有效性检测
	  if(Pinfo->circ_r2>0)
	  {
			//圆弧半径必须满足一定范围
			if(((Pinfo->circ_r2)<10000)&&((Pinfo->circ_r2)>4))
			{
				xx_t = Pinfo->circ_x;
				yy_t = Pinfo->circ_y;
				circle_l = xx_t*xx_t+yy_t*yy_t;
				circle_l = circle_l/100;
				mid_l = Pinfo->mid_x*Pinfo->mid_x+Pinfo->mid_y*Pinfo->mid_y;
				mid_l = mid_l/100;
				// %圆心距离原点坐标必须大于数据段中点距离原点坐标                 
				if(circle_l<mid_l)
				{
					Pinfo->circ_x = 0;
					Pinfo->circ_y = 0;
					Pinfo->circ_r2 = 0;
				}
				// %圆心距离原点距离必须大于半径
				if(circle_l<Pinfo->circ_r2)
				{
					Pinfo->circ_x = 0;
					Pinfo->circ_y = 0;
					Pinfo->circ_r2 = 0;
				}
			}
			else
			{
				Pinfo->circ_x = 0;
				Pinfo->circ_y = 0;
				Pinfo->circ_r2 = 0;
			}
	  }
	  //识别结果存入缓存
		if(Pinfo->circ_r2>0){
			Seg_Circ_i = Queue_T->size;
			Queue_T->Queue[Seg_Circ_i].circ_x = Pinfo->circ_x;
			Queue_T->Queue[Seg_Circ_i].circ_y = Pinfo->circ_y;
			Queue_T->Queue[Seg_Circ_i].circ_r2 = Pinfo->circ_r2;
			Queue_T->Queue[Seg_Circ_i].circ_r2_bias = Pinfo->circ_r2_bias;
			Queue_T->Queue[Seg_Circ_i].circ_points = Pinfo->circ_points;
			Queue_T->Queue[Seg_Circ_i].mid_x = Pinfo->mid_x;
			Queue_T->Queue[Seg_Circ_i].mid_y = Pinfo->mid_y;
			Queue_T->size++;
//			usart1Printf("index->%d, circ_x->%d, circ_y->%d, circ_r2(cm^2)->%d \n", 
//										Seg_Circ_i, Pinfo->circ_x, Pinfo->circ_y, Pinfo->circ_r2);	
		}
	  // %循环退出检测
	  if ((k+offset)>index_e){break;}
	}
}
//圆心距离检测
static void CircDisCheck(Sick_Queue* Queue_T)
{
	int16_t *Pseg_x, *Pseg_y, *Pseg_r2, *Pseg_r2_bias;
	int16_t xx_t, yy_t;
	int32_t circ_dis2, r2_t;
	uint16_t num, i, m, n;
	
	Pseg_x = Seg_Circ_X;
	Pseg_y = Seg_Circ_Y;
	Pseg_r2 = Seg_Circ_R2;
	Pseg_r2_bias = Seg_Circ_R2_Bias;
	
	
	num = Queue_T->size;
	if(num>80)num = 80;
	//data copy
	for (i=0;i<num; i++)
	{
		Pseg_x[i] = Queue_T->Queue[i].circ_x;
		Pseg_y[i] = Queue_T->Queue[i].circ_y;
		Pseg_r2[i] = Queue_T->Queue[i].circ_r2;
		Pseg_r2_bias[i] = Queue_T->Queue[i].circ_r2_bias;
	}
	// %识别圆心距离检测
	//usart1Printf("num->:%d \n",  num);
	if(num>=2)
	{
		//圆心距离计算
		for (m=0;m<(num-1);m++)
		{
			if(Pseg_r2[m] >0)
			{
				for (n=(m+1);n<num; n++)
				{
					if(Pseg_x[n]>0)
					{
						xx_t = (Pseg_x[m]-Pseg_x[n]);
						yy_t = (Pseg_y[m]-Pseg_y[n]);
						circ_dis2 = xx_t*xx_t+yy_t*yy_t;
						circ_dis2 = circ_dis2/100;
						r2_t = (Pseg_r2[m]+Pseg_r2[n]);
						// %圆心距离小于半径和距离
						if(circ_dis2<r2_t)
						{
							if(Pseg_r2_bias[m]>Pseg_r2_bias[n])
							{
								// %无效圆心清零
								//usart1Printf("clear->:%d \n",  n);
								Pseg_x[n] = 0;
								Pseg_y[n] = 0;
								Pseg_r2[n] = 0;
								Pseg_r2_bias[n] = 0;
							}
							else
							{
								// %无效圆心清零
								//usart1Printf("clear->:%d \n",  m);
								Pseg_x[m] = 0;
								Pseg_y[m] = 0;
								Pseg_r2[m] = 0;
								Pseg_r2_bias[m] = 0;
								// %退出本轮循环
								break;
							}
								
						}
					}
				}
			}
		}
	}
	//data update
	for (i=0;i<num; i++)
	{
		Queue_T->Queue[i].circ_x = Pseg_x[i];
		Queue_T->Queue[i].circ_y = Pseg_y[i];
		Queue_T->Queue[i].circ_r2 = Pseg_r2[i];
		Queue_T->Queue[i].circ_r2_bias = Pseg_r2_bias[i];
	}
}
static void CircSearch_Mgt(DataQueue* DataQueue_T, Sick_Queue* Queue_R)
{
	uint16_t i, p, circl_num, Circ_i;
	uint16_t len, index1, index2, offset, offset_div, offset_t;
	Sick_Queue *Pqueue;
	Pqueue = &Queue_T;
	
	Circ_i = 0;
	len = DataQueue_T->Seg_Len;
	for(i=0;i<len; i++)
	{
		index1=DataQueue_T->SegPoints[i].Sn_S;
		index2=DataQueue_T->SegPoints[i].Sn_E-2;
		offset = index2-index1;
//		usart1Printf("/***************************************i->%d, Start->%d, End->%d, offset->%d \n", 
//									i, index1, index2, offset);	
//		usart1Printf("i->%d, Start->%d, End->%d, offset->%d \n", 
//									i, index1, index2, offset);	
		for(offset_div=1;offset_div<80;offset_div++)
		{
			if((offset/offset_div)>10)
			{
				offset_t = (uint8_t)(offset/offset_div);
				// %数据段识别圆心次数数统计
				SegDataCircSearch(index1, index2, offset_t, DataQueue_T, Pqueue);				
				//识别圆心距离检测
				CircDisCheck(Pqueue);
				//有效圆心统计
				circl_num = Pqueue->size;
				//usart1Printf("circl_num->%d \n",  circl_num);	

				for (p=0;p<circl_num;p++)
				{
					if(Pqueue->Queue[p].circ_r2>0)
					{
						Queue_R->Queue[Circ_i].circ_x = Pqueue->Queue[p].circ_x;
						Queue_R->Queue[Circ_i].circ_y = Pqueue->Queue[p].circ_y;
						Queue_R->Queue[Circ_i].circ_r2 = Pqueue->Queue[p].circ_r2;
						Queue_R->Queue[Circ_i].circ_r2_bias = Pqueue->Queue[p].circ_r2_bias;
						Queue_R->Queue[Circ_i].circ_points = Pqueue->Queue[p].circ_points;
						Queue_R->Queue[Circ_i].mid_x = Pqueue->Queue[p].mid_x;
						Queue_R->Queue[Circ_i].mid_y = Pqueue->Queue[p].mid_y;
						Circ_i++;						
					}
				}
				//usart1Printf("circl_num->%d, total->%d \n",  circl_num, Circ_i);	
			}	
		}
	}
	Queue_R->size = Circ_i;
}

	/*******************************************************************
	*para StartAngle: which angle begin
	*para Data: first data is the valid data num for the list
	*return : Struct
	*Note: angle convert to sn num, -45~225->0~811
	******************************************************************/
void Sick_Circle_Init(void)
{
	Data_Input.Queue.Data = QData;//ValidData_D;//QData;
	Data_Input.Queue.Sn = QSn;
	Data_Input.DivPoints = DivPoints;
	Data_Input.XY_Queue.X = Points_X;
	Data_Input.XY_Queue.Y = Points_Y;
	EthQueue.head = 0;
	EthQueue.size = 0;
	EthQueue.tail = 0;
}


//void Sick_Circle_Mgt(int16_t* Data_In, uint16_t StartAngle)
void Sick_Circle_DataMgt(EthInfo* Pinfo)
{	
	int16_t* Pdata;
	uint16_t len, i;
	uint16_t StartAngle = 0;
	//Pdata = &ValidData_D[0];
	Pdata = Pinfo->polardata;
	//StartAngle = (0xFFFFFFFF-Pinfo->startangle+1)/1000;//Ratio:1:100
	//usart1Printf("Begin. \n");
	//aviod recheck the same data
	if(0==Data_Input.IsCheck){
		Sick_Result.IsSet = 0;
	}
	else
	{
		return;
	}	
	
	Data_Input.size = 0;
	Sick_Result.size = 0;
	//the Data_In[0] is the len of the data
	len = Pdata[0];	
	Data_Input.size = len;
	for(i=0;i<len;i++)
	{
		Data_Input.Queue.Data[i] = Pdata[i+1];
		Data_Input.Queue.Sn[i] = i+StartAngle;
		Data_Input.XY_Queue.X[i] = 0;
		Data_Input.XY_Queue.Y[i] = 0;
		//usart1Printf("Sn.%d \n", Data_Input.Queue.Sn[i]);
	}
	//usart1Printf("Data Set. \n");
	
	//Range Set, update DataQueue_T , which not meet the range area would be ignore
	SearchArea_Mgt(&Data_Input, Max_Len);
	//usart1Printf("Range Set. \n");
	
	//Continue Set, add the div info to the strcut
	Continue_Mgt(&Data_Input);
	//usart1Printf("Continue Set. \n");
	
	//Valid Segment data set, add seg info to the struct
	Valid_SegMent_Mgt(&Data_Input, MinSegNum);
	//usart1Printf("Valid Segment Set. \n");
	
	//coordinate convert, from polar to coordinate
	CoordinateConvert_Mgt(&Data_Input);
	//usart1Printf("Coordinate convert Set. \n");
	
	//Circle search
	CircSearch_Mgt(&Data_Input, &Sick_Result);
	

	Sick_Result.IsSet = 1;
	Data_Input.IsCheck = 1;
	usart1Printf("End:%d \n", Sick_Result.size);
}

void Sick_Circle_Mgt(void)
{
	uint8_t pos;
	EthInfo* Pinfo;
	if(EthQueue.size>0){
		pos = EthQueue.tail;
		Pinfo = &(EthQueue.queue[pos]);
		Sick_Circle_DataMgt(Pinfo);
		EthQueue.size--;
		if(pos<(EthQueue_Max-1)){
			EthQueue.tail++;
		}else{
			EthQueue.tail = 0;
		}
	}
}
